pub mod mv3drgbd;
use mv3drgbd::*;
use crate::Mv3dRgbdDeviceType::*;
use std::ffi::c_void;
use chrono::Local;

/// 定义类型：回调产生的图像数据类型。
type FrameCallback = Box<dyn FnMut(&MV3D_RGBD_FRAME_DATA) + Send>;

/// 相机结构体包装器
/// * handle: 相机句柄
/// * frame_callback: 回调产生的图像数据
/// * sdk_version: SDK版本
/// * dev_name: 设备名称
pub struct RGBDCamera {
    handle: HANDLE,
    frame_callback: Option<FrameCallback>,
    sdk_version: String,
    dev_name: String,
}

/// # 相机各种操作
/// 使用结构体方法实现相机的各种操作。
impl RGBDCamera {
    /// 创建新的相机实例
    pub fn new() -> Result<Self, String> {
        unsafe {
            // 获取SDK版本信息
            let mut version_info = MV3D_RGBD_VERSION_INFO::default();
            check_status(
                MV3D_RGBD_GetSDKVersion(&mut version_info),
                "获取SDK版本"
            )?;
            
            println!("\x1b[36m[SDK版本] {}.{}.{}\x1b[0m", 
                version_info.nMajor,
                version_info.nMinor,
                version_info.nRevision);

            let version = format!("{}.{}.{}", version_info.nMajor, version_info.nMinor, version_info.nRevision);

            // 初始化SDK
            check_status(MV3D_RGBD_Initialize(), "初始化SDK")?;

            Ok(RGBDCamera {
                handle: std::ptr::null_mut(),
                frame_callback: None,
                sdk_version: version,
                dev_name: String::from("default"),
            })
        }
    }

    /// 打开默认[第一个]相机
    pub fn open(&mut self) -> Result<(), String> {
        unsafe {
            // 获取设备数量
            let mut dev_num: u32 = 0;
            let device_types = DeviceType_Ethernet as u32 |
                DeviceType_USB as u32 |
                DeviceType_Ethernet_Vir as u32 |
                DeviceType_USB_Vir as u32;
            check_status(
                MV3D_RGBD_GetDeviceNumber(
                    device_types,
                    &mut dev_num,
                ),
                "获取设备数量"
            )?;
            
            if dev_num == 0 {
                return Err("未找到设备".to_string());
            }

            // 获取设备列表
            let mut dev_list = vec![MV3D_RGBD_DEVICE_INFO::default(); dev_num as usize];
            check_status(
                MV3D_RGBD_GetDeviceList(
                    device_types,
                    dev_list.as_mut_ptr(),
                    dev_num,
                    &mut dev_num,
                ),
                "获取设备列表"
            )?;

            // 将设备名称转换为字符串
            let device_name = std::ffi::CStr::from_ptr(dev_list[0].chUserDefinedName.as_ptr())
                .to_string_lossy()
                .trim()
                .to_owned();
            println!("\x1b[36m[默认设备名称] {}\x1b[0m", device_name);

            // 打开第一个设备
            let mut handle = std::ptr::null_mut();
            check_status(
                MV3D_RGBD_OpenDevice(&mut handle, &mut dev_list[0]),
                "打开设备"
            )?;
            self.handle = handle;
            self.dev_name = device_name;
        }
        Ok(())
    }

    /// 主动获取一帧图像
    pub fn fetch_frame(&self, timeout_ms: u32) -> Result<MV3D_RGBD_FRAME_DATA, String> {
        unsafe {
            let mut frame_data = MV3D_RGBD_FRAME_DATA::default();
            check_status(
                MV3D_RGBD_FetchFrame(self.handle, &mut frame_data, timeout_ms),
                "获取图像"
            )?;
            Ok(frame_data)
        }
    }

    /// 注册回调函数获取图像
    pub fn register_frame_callback<F>(&mut self, callback: F) -> Result<(), String>
    where
        F: FnMut(&MV3D_RGBD_FRAME_DATA) + Send + 'static,
    {
        unsafe {
            // 保存回调函数
            self.frame_callback = Some(Box::new(callback));
            
            // 创建C回调函数
            extern "C" fn callback_wrapper(frame: *mut MV3D_RGBD_FRAME_DATA, user_param: *mut c_void) {
                let camera = user_param as *mut RGBDCamera;
                if !camera.is_null() {
                    unsafe {
                        if let Some(callback) = (*camera).frame_callback.as_mut() {
                            callback(&*frame);
                        }
                    }
                }
            }

            // 注册回调函数
            check_status(
                MV3D_RGBD_RegisterFrameCallBack(
                    self.handle,
                    callback_wrapper,
                    self as *mut _ as *mut c_void,
                ),
                "注册回调函数"
            )
        }
    }

    /// 深度图像转换点云图像
    pub fn depth_to_pointcloud(&mut self, depth_image: &MV3D_RGBD_IMAGE_DATA) -> Result<MV3D_RGBD_IMAGE_DATA, String> {
        unsafe {
            let mut point_cloud = MV3D_RGBD_IMAGE_DATA::default();

            check_status(
                MV3D_RGBD_MapDepthToPointCloud(
                    self.handle,
                    depth_image as *const _ as *mut _,
                    &mut point_cloud as *mut _
                ),
                "深度图像转换点云图像"
            )?;
            Ok(point_cloud)
        }
    }

    /// 获取点云数据中指定像素点的三维坐标
    pub fn get_point_xyz(&self, point_cloud: &MV3D_RGBD_IMAGE_DATA, x: u32, y: u32) 
        -> Result<(f32, f32, f32), String> 
    {
        if x >= point_cloud.nWidth || y >= point_cloud.nHeight {
            return Err(format!("像素坐标({}, {})超出范围 {}x{}", 
                x, y, point_cloud.nWidth, point_cloud.nHeight));
        }

        let point_idx = (y * point_cloud.nWidth + x) as usize;
        
        unsafe {
            let points = std::slice::from_raw_parts(
                point_cloud.pData as *const f32,
                (point_cloud.nDataLen / 4) as usize
            );

            let idx = point_idx * 3;
            if idx + 2 < points.len() {
                Ok((
                    points[idx],     // X 坐标
                    points[idx + 1], // Y 坐标
                    points[idx + 2]  // Z 坐标
                ))
            } else {
                Err("点云数据索引越界".to_string())
            }
        }
    }

    /// 获取SDK版本信息
    pub fn get_sdk_version(&self) -> &str {
        &self.sdk_version
    }

    /// 获取设备名称
    pub fn get_device_name(&self) -> &str {
        &self.dev_name
    }

    /// 开始工作
    pub fn start(&self) -> Result<(), String> {
        unsafe {
            check_status(MV3D_RGBD_Start(self.handle), "启动采集")
        }
    }

    /// 停止工作
    pub fn stop(&self) -> Result<(), String> {
        unsafe {
            check_status(MV3D_RGBD_Stop(self.handle), "停止采集")
        }
    }

    /// 关闭相机
    pub fn close(&mut self) -> Result<(), String> {
        unsafe {
            if !self.handle.is_null() {
                check_status(MV3D_RGBD_CloseDevice(&mut self.handle), "关闭设备")?;
                self.handle = std::ptr::null_mut();
            }
            Ok(())
        }
    }

}

impl Drop for RGBDCamera {
    fn drop(&mut self) {
        let _ = self.close();
        unsafe {
            MV3D_RGBD_Release();
        }
    }
}

// 为了安全使用，实现Send和Sync
unsafe impl Send for RGBDCamera {}
unsafe impl Sync for RGBDCamera {}

/// 状态检查函数
/// 输出时间戳，成功信息为绿色，错误信息为红色；
/// 输出错误信息所代表的意思。
fn check_status(status: MV3D_RGBD_STATUS, operation: &str) -> Result<(), String> {
    // 获取当前时间戳
    let timestamp = Local::now().format("%Y-%m-%d %H:%M:%S%.3f");
    
    if status == MV3D_RGBD_OK {
        // 成功信息显示为绿色
        println!("\x1b[32m[{}] ✓ {}\x1b[0m", timestamp, operation);
        Ok(())
    } else {
        // 错误信息显示为红色
        let error_msg = match status {
            MV3D_RGBD_E_HANDLE => "错误或无效的句柄",
            MV3D_RGBD_E_SUPPORT => "不支持的功能",
            MV3D_RGBD_E_BUFOVER => "缓存溢出",
            MV3D_RGBD_E_CALLORDER => "函数调用顺序错误",
            MV3D_RGBD_E_PARAMETER => "参数错误",
            MV3D_RGBD_E_RESOURCE => "资源申请失败",
            MV3D_RGBD_E_NODATA => "无数据",
            MV3D_RGBD_E_PRECONDITION => "前置条件有误，或运行环境已发生变化",
            MV3D_RGBD_E_VERSION => "版本不匹配",
            MV3D_RGBD_E_NOENOUGH_BUF => "传入的内存空间不足",
            MV3D_RGBD_E_ABNORMAL_IMAGE => "异常图像，可能是丢包导致图像不完整",
            MV3D_RGBD_E_LOAD_LIBRARY => "动态导入DLL失败",
            MV3D_RGBD_E_ALGORITHM => "算法错误",
            MV3D_RGBD_E_DEVICE_OFFLINE => "设备离线",
            MV3D_RGBD_E_ACCESS_DENIED => "设备无访问权限",
            MV3D_RGBD_E_OUTOFRANGE => "值超出范围",
            MV3D_RGBD_E_UPG_FILE_MISMATCH => "升级固件不匹配",
            MV3D_RGBD_E_UPG_CONFLICT => "升级冲突",
            MV3D_RGBD_E_UPG_INNER_ERR => "升级时相机内部出现错误",
            MV3D_RGBD_E_INDUSTRY => "行业属性不匹配",
            MV3D_RGBD_E_NETWORK => "网络相关错误",
            _ => "未知错误",
        };
        println!("\x1b[31m[{}] ✗ {} 失败: {} (错误码: 0x{:X})\x1b[0m", 
                timestamp, operation, error_msg, status);
        Err(format!("{} 失败: {} (错误码: 0x{:X})", operation, error_msg, status))
    }
}


/// 将 YUV422 格式的图像数据转换为 RGB 格式
pub fn convert_yuv422_to_rgb(yuv_data: &[u8], width: u32, height: u32) -> Vec<u8> {
    let mut rgb_data = vec![0u8; (width * height * 3) as usize];
    let mut out = 0;

    for in_idx in (0..(width * height * 2)).step_by(4) {
        let in_idx = in_idx as usize;
        if in_idx + 3 >= yuv_data.len() {
            break;
        }

        // 从YUV数据中提取像素值
        let pixel_16 = ((yuv_data[in_idx + 3] as u32) << 24)
            | ((yuv_data[in_idx + 2] as u32) << 16)
            | ((yuv_data[in_idx + 1] as u32) << 8)
            | (yuv_data[in_idx] as u32);

        let y0 = (pixel_16 & 0x000000ff) as i32;
        let u = ((pixel_16 & 0x0000ff00) >> 8) as i32;
        let y1 = ((pixel_16 & 0x00ff0000) >> 16) as i32;
        let v = ((pixel_16 & 0xff000000) >> 24) as i32;

        // 转换第一个像素
        let (r, g, b) = yuv_to_rgb(y0, u, v);
        if out + 2 < rgb_data.len() {
            rgb_data[out] = r;
            rgb_data[out + 1] = g;
            rgb_data[out + 2] = b;
            out += 3;
        }

        // 转换第二个像素
        let (r, g, b) = yuv_to_rgb(y1, u, v);
        if out + 2 < rgb_data.len() {
            rgb_data[out] = r;
            rgb_data[out + 1] = g;
            rgb_data[out + 2] = b;
            out += 3;
        }
    }

    rgb_data
}


/// YUV 转 RGB 的颜色空间转换
fn yuv_to_rgb(y: i32, u: i32, v: i32) -> (u8, u8, u8) {
    let r = y + ((1.370705 * (v - 128) as f64) as i32);
    let g = y - ((0.698001 * (v - 128) as f64) as i32) - ((0.337633 * (u - 128) as f64) as i32);
    let b = y + ((1.732446 * (u - 128) as f64) as i32);

    (
        r.clamp(0, 255) as u8,
        g.clamp(0, 255) as u8,
        b.clamp(0, 255) as u8
    )
}

/// 将深度图数据转换为带色阶的 RGB 格式
pub fn convert_depth_to_rgb(depth_data: &[u8], width: u32, height: u32) -> Vec<u8> {
    let mut rgb_data = vec![0u8; (width * height * 3) as usize];
    let depth_values = unsafe { std::slice::from_raw_parts(
        depth_data.as_ptr() as *const i16,
        (width * height) as usize
    )};

    // 找出深度的最大值和最小值
    let mut min_depth = i16::MAX;
    let mut max_depth = i16::MIN;
    for &depth in depth_values.iter() {
        if depth <= 0 { // 修改：包括负值和零值都跳过
            continue;
        }
        if depth > max_depth {
            max_depth = depth;
        }
        if depth < min_depth {
            min_depth = depth;
        }
    }

    let depth_range = if max_depth > min_depth {
        (max_depth - min_depth) as f32
    } else {
        1.0 // 防止除以零
    };
    let mut out_idx = 0;

    for depth in depth_values.iter() {
        if out_idx + 2 >= rgb_data.len() {
            break;
        }

        if *depth <= 0 { // 修改：包括负值和零值都视为无效
            rgb_data[out_idx] = 0;
            rgb_data[out_idx + 1] = 0;
            rgb_data[out_idx + 2] = 0;
        } else {
            // 计算归一化的深度值 (0.0 - 1.0)
            let normalized_depth = (*depth - min_depth) as f32 / depth_range;
            let normalized_depth = normalized_depth.clamp(0.0, 1.0);

            // 使用 JET 颜色映射
            let (r, g, b) = jet_colormap(normalized_depth);
            rgb_data[out_idx] = r;
            rgb_data[out_idx + 1] = g;
            rgb_data[out_idx + 2] = b;
        }
        out_idx += 3;
    }

    rgb_data
}

/// JET 颜色映射函数
fn jet_colormap(v: f32) -> (u8, u8, u8) {
    let v = v.clamp(0.0, 1.0);
    let r = (1.5 - f32::abs(2.0 * v - 1.0)).clamp(0.0, 1.0);
    let g = (1.5 - f32::abs(2.0 * v - 0.5)).clamp(0.0, 1.0);
    let b = (1.5 - f32::abs(2.0 * v - 0.0)).clamp(0.0, 1.0);
    
    ((r * 255.0) as u8, (g * 255.0) as u8, (b * 255.0) as u8)
}